| 【精选】ORBSLAM2中的共视图、本征图和生成树 | 您所在的位置：网站首页 › essentials 镭射 › 【精选】ORBSLAM2中的共视图、本征图和生成树 |
【精选】ORBSLAM2中的共视图、本征图和生成树
|
目录 共视图、生成树、本征图的关系 1. 相同点 2.不同点 代码与粗略理解 Covisibility Graph 建立过程 应用场所 Essential Graph Spanning tree 共视图、生成树、本征图的关系
他们都是以关键帧作为节点，帧之间建立一条边的图模型。 2.不同点1）共视图(Covisibility Graph)最稠密，本征图(Essential Graph)次之，生成树(Spanning tree)最稀疏 2）共视图保存了关键帧与所有共视关键点大于某一阈值的的共视帧之间的关系 3）本征图包含生成树的链接关系、形成闭环的连接关系、共视关系非常好的关键帧连接关系(共视关键点>=100) 4）生成树只包含了关键帧与附近共视关系最好的关键帧，即下面要说的父子关系。 代码与粗略理解要理解这三个东西主要去到ORBSLAM2/src/keyframe.cc里面的KeyFrame::UpdateConnections() 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度 对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高） 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树 这里的代码注释建议仔细看看，是程小六大佬写的。 要理解更好，建议去看大佬的视频。 void KeyFrame::UpdateConnections() { // 关键帧-权重，权重为其它关键帧与当前关键帧共视地图点的个数，也称为共视程度 map KFcounter; vector vpMP; { // 获得该关键帧的所有地图点 unique_lock lockMPs(mMutexFeatures); vpMP = mvpMapPoints; } // Step 1 通过地图点被关键帧观测来间接统计关键帧之间的共视程度 // 统计每一个地图点都有多少关键帧与当前关键帧存在共视关系，统计结果放在KFcounter //遍历本关键帧的所有地图点 for(vector::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++) { MapPoint* pMP = *vit; if(!pMP) continue; if(pMP->isBad()) continue; // 对于每一个地图点，observations记录了可以观测到该地图点的所有关键帧 map observations = pMP->GetObservations(); //对于一个地图点，遍历观测共视帧。1帧的共视地图点增1. for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) { // 除去自身，自己与自己不算共视 if(mit->first->mnId==mnId) continue; // 这里的操作非常精彩！ // map[key] = value，当要插入的键存在时，会覆盖键对应的原来的值。如果键不存在，则添加一组键值对 // mit->first 是地图点看到的关键帧，同一个关键帧看到的地图点会累加到该关键帧计数 // 所以最后KFcounter 第一个参数表示某个关键帧，第2个参数表示该关键帧看到了多少当前帧的地图点，也就是共视程度 KFcounter[mit->first]++; } } // 没有共视关系，直接退出 if(KFcounter.empty()) return; int nmax=0; // 记录最高的共视程度 KeyFrame* pKFmax=NULL; // 至少有15个共视地图点才会添加共视关系 int th = 15; // vPairs记录与其它关键帧共视帧数大于th的关键帧 // pair将关键帧的权重写在前面，关键帧写在后面方便后面排序 vector vPairs; vPairs.reserve(KFcounter.size()); // Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧） for(map::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { if(mit->second>nmax) { nmax=mit->second; pKFmax=mit->first; } // 建立共视关系至少需要大于等于th个共视地图点 if(mit->second>=th) { // 对应权重需要大于阈值，对这些关键帧建立连接 vPairs.push_back(make_pair(mit->second,mit->first)); // 对方关键帧也要添加这个信息 // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重 (mit->first)->AddConnection(this,mit->second); } } // Step 3 如果没有超过阈值的权重，则对权重最大的关键帧建立连接 if(vPairs.empty()) { // 如果每个关键帧与它共视的关键帧的个数都少于th， // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights // 这是对之前th这个阈值可能过高的一个补丁 vPairs.push_back(make_pair(nmax,pKFmax)); pKFmax->AddConnection(this,nmax); } // Step 4 对满足共视程度的关键帧对更新连接关系及权重（从大到小） // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序 sort(vPairs.begin(),vPairs.end()); // sort函数默认升序排列 // 将排序后的结果分别组织成为两种数据类型 list lKFs; list lWs; for(size_t i=0; iAddChild(this); mbFirstConnection = false; } } } Covisibility Graph 建立过程1.通过地图点被关键帧观测来间接统计关键帧之间的共视程度 统计关键帧的共视关系很有趣：先建立一个KeyFrame到权重的映射表。然后遍历当前关键帧的所有看到的地图点，对每一个地图点，再遍历器观测的关键帧。对于一个地图点看到的一个关键帧，在映射表里的权重加1。 2. 找到对应权重最大的关键帧（共视程度最高的关键帧） 判断之前要求关键帧共视点大于15个 3. 如果没有超过阈值的权重，则对权重最大的关键帧建立连接 到这一步其实共视图已经完成，后面是完成生成树的。 4. 对满足共视程度的关键帧对更新连接关系及权重（从大到小） 排序 5. 更新生成树的连接 以当前节点为儿子，当前节点共视关系最大的帧作为父节点(一个子节点只能有一个父节点，一个父节点可以有多个子节点)。 应用场所1. 跟踪局部地图，扩大搜索范围：Tracking::UpdateLocalKeyFrames() 在跟踪线程中，只凭一个关键帧跟踪的地图点比较少。可以将附近关键帧的共视帧看到的地图点一起放进去，可以防止跟丢。 2. localmapping线程里关键帧之间新建、融合地图点 LocalMapping::CreateNewMapPoints() tracking线程里面跟踪的点可能越来越少，所以要加新的。 LocalMapping::SearchInNeighbors() 把共视帧比较近的地图点融合为一点。 3. 闭环检测、重定位检测 • LoopClosing::DetectLoop() 、 LoopClosing::CorrectLoop() • KeyFrameDatabase::DetectLoopCandidates • KeyFrameDatabase::DetectRelocalizationCandidates 判断是否达到回环，只有一个关键帧不够，要将其的共视帧一起拉下判断。 Essential Graph 本质图比共视图更稀疏，这是因为本质图的作用是用在 闭环矫正 时，用相似变换来矫正尺度漂移，把闭环误差均摊在本质图中。本质图中节点也是所有关键帧，但是连接边更少，只保留了联系 紧密的边来使得结果更精确。
当本质图用来全局优化时，会有更好的精度和更短时间。不过请注意，本质图只优化位姿；而全局BA即优化位姿又优化地图点。因此在ORBSLAM2里使用了本质图加full BA的方法。(这里的 
Spanning tree
Spanning tree
只保留与关键帧关系最近的共视帧，因此，每个节点之间都是父子关系。 |
【本文地址】